perm filename ARMSW[SYS,HE] blob
sn#064720 filedate 1973-10-01 generic text, type T, neo UTF8
COMMENT ⊗ VALID 00017 PAGES
RECORD PAGE DESCRIPTION
00001 00001
00003 00002 IFN USER+TRACK,<0>
00006 00003 NXTJIF:
00008 00004 WATSTL: CONI CLOCK,AC
00010 00005 MAIN: TRNE RUN
00013 00006 TT: PUSHJ P,REFLEX
00014 00007 ANGLES: MOVEI I,5
00017 00008 IFE USER,<
00018 00009 SERVO: TRNN RUN
00020 00010 DRIVE:
00023 00011 IFN SIMU,< IF IN SIMU PREDICT NEXT VALUE OF THETA
00025 00012 NXTH: MOVE AC,[XWD T0,T0P]
00028 00013 EVAL: MOVEI I,5
00033 00014 DHO: AOJ I, DISASTER HAS OCCURED
00036 00015 NEXT: TRNE RUN
00040 00016 PREAD:
00042 00017 NON: POINT 12,INNER(AC),11
00047 ENDMK
⊗;
IFN USER+TRACK,<0>
STRADR: MOVE BITSAV ;Enters here every 1/60 th. seccond
IFE USER+TRACK,<
TRNE DTST
CALLI 400024
DATAO ARM,[2]
SOSLE STWT
CALLI 400024
>
MOVE P,[XWD -PDLL,PDL] ;PUSH DOWN STACK
MOVE CBUF,CBUFS
MOVE DATWD,DATWDS
IORI DATWD,2 ;YELLOW ARM
IFN REPORT+TRACK,<
MOVE DATA,DATPT
>
IFN USER+TRACK,<
TRNE DTST
JRST SWDONE
>
IFE USER,<
IFN TRACK,<
SKIPE WALK
JRST NCHK
>
CONSZ 40 ;CHECK TO SEE IF ON PDP-6
JRST[ MOVEI TAC,PDPERR
MOVEM TAC,TRAJER
JRST TOFF]
CONI 0,AC
ANDI AC,7
CONO 0,2050(AC)
>
TRZE TRIP ;TURN ON ARM
JRST[
DATAO ARM,[3]
NCHK: MOVEI K,51B23 ;-10 VOLT REFERENCE
PUSHJ P,PREAD ;REFTR← -10 volt reference
LDB AC,SNUM ;PICK UP MIDDLE SAMPLE
ANDI DACVAL,7777 ;LEAVE LAST SAMPLE IN J
ADDI AC,(DACVAL) ;ADD THEM TOGETHER
CAIGE AC,17000 ;IS THE FUSE BLOWN
JRST[ MOVEI TAC,REFERR
MOVEM TAC,TRAJER
JRST TOFF]
MOVEM AC,REFTR ;REFERENCE READING
MOVEI I,2 ;FIRST THREE TACHS
MOVE K,VCHAN(I)
ZVEL: PUSHJ P,PREAD
LDB AC,SNUM
ANDI DACVAL,7777
ADDI AC,(DACVAL)
MOVEM AC,VZ(I)
SOJGE I,ZVEL
JRST .+1]
IFN TRACK,<
SKIPE WALK
JRST TOK
>
MOVEI AC,=16666
MOVEM AC,ETIME
MOVSI AC,(1.0)
MOVEM AC,TDF
IFE USER,<
CONI CLOCK,AC
TLZ AC,777774
JRST STIME ;TIME IN MICRO SECONDS
>
IFN USER,<JRST .+2>
NXTJIF:
IFN USER,<JRST SWDONE>
IFN DEB<
MOVEM 17,ACSAV+17
MOVEI 17,ACSAV
BLT 17,ACSAV+16
MOVE 17,ACSAV+17
>
IFN TRACK,<
SKIPE WALK
JRST TOK
>
IFN REPORT+TRACK,<
HRRZI TAC,(DATA)
CAIL TAC,DATO+DBL-1
JRST[ SUB DATA,[XWD DBL,DBL]
MOVE TAC,[XWD DATO+DBL,DATO]
BLT TAC,(DATA)
HRRZI AC,(DATA)
HRRZ TAC,DATIWD
SUBI AC,(TAC)
JRST RDP]
HRRZI AC,(DATA)
HRRZ TAC,DATIWD
SUBI AC,(TAC)
HRRZ MQ,DATPT
SUBI TAC,(MQ)
IMUL AC,TAC
RDP: SKIPLE AC
IFN TRACK,<PUSH DATA,[0]>
IFN REPORT,<PUSH DATA,[SIXBIT/ERROR/]>
MOVEI AC,DATO-1
HRRZ TAC,DATIWD
CAIL TAC,DATO+DBL-1
HRRM AC,DATIWD
MOVEM DATA,DATPT
>
IFE USER,<
MOVEI TAC,=2000
WATSTL: CONI CLOCK,AC
TLZ AC,777774
SUB AC,LTIME
JUMPGE AC,.+2
ADD AC,[=1000000]
IFN REPORT,<
CAIN TAC,=2000
JRST[ PUSH DATA,[<SIXBIT/TICK/>+1]
PUSH DATA,AC
JRST .+1]
>
MOVEM AC,ETIME
TRNE RUN
JRST[ MOVE MQ,AC
FSC MQ,233
FMPR MQ,[0.00006]
MOVEM MQ,TDF
MOVE AC,LTIME
ADD AC,ETIME
JRST SSTIME]
CAIGE AC,=16667 ;delta T greater than 16.7 msec.?
SOJGE TAC,WATSTL
JUMPL TAC,TE
MOVSI AC,(1.0)
MOVEM AC,TDF
MOVE AC,LTIME
ADDI AC,=16667
SSTIME: TLNE AC,777774
SUB AC,[=1000000]
STIME: MOVEM AC,LTIME ;LTIME←LTIME+16.7 msec.
>
SOSGE COUNT
JRST[TE:MOVEI AC,TERR
MOVEM AC,TRAJER
JRST ZITO]
MAIN: TRNE RUN
JRST[ PUSHJ P,EVAL
PUSHJ P,NXTH
JRST .+1]
MOVEI J,6
PUSHJ P,ANGLES ;read joint angles
TRNE RONLY ;read angles only?
JRST[ PUSHJ P,HEAD ;read hand also
MOVE AC,HAND
MOVEM AC,SET
PUSHJ P,REFLEX
PUSHJ P,READED] ;DOES NOT RETURN
TRNN RUN
JRST[ SKIPG TAC,FUNCT ;not run, mabye a function
PUSHJ P,SETSET
JUMPL TAC,NEXT
JUMPE TAC,[CAIL CBUF,BUF;try next buffer
SETOM (CBUF) ;dont do this if first buffer
MOVEM CBUF,CBUFS
ADDI CBUF,1001
CAILE CBUF,BUF2
MOVEI CBUF,BUF ;Advance buffer
MOVE 2,(CBUF)
JUMPL 2,[BUFWT:MOVE CBUF,CBUFS;wait for buffer
JRST SWDONE]
JUMPN 2,BUFRDY ;the last buffer stop.
JRST[ TRO DTST
MOVEI 2,WAITIN
HRRM 2,JUMPBK
MOVEM 2,STKPTR
IFN REPORT,<PUSH DATA,[0]>
JRST BUFWT]
BUFRDY: HRRZ 2,(CBUF)
ADDI 2,-1(CBUF)
HRRZM 2,STKPTR
JRST NEXT]
CAIN TAC,6
JRST SETCH
JRST TT] ;otherwise perform function
JUMPE J,[RUDONE:
TRNN NSET
PUSHJ P,SETSET
TRZ DROP+RUN+FINAL+WOB ;if J is zero then end of run
TDZ DATWD,[770001252500]
TRZE STP
JRST[ MOVEI TAC,STPERR
MOVEM TAC,TRAJER
POP P,TAC
JRST TOFF]
SKIPN AC,FUNCT
JRST NEXT
CAIN AC,22
JRST[ TRZ HCL
SETZM TDAT
SETZM FUNCT
DATAO DEVT,TDAT
JRST NEXT]
MOVEI AC,=16667
MOVEM AC,ETIME
JRST .+1]
TT: PUSHJ P,REFLEX
DISP: MOVE AC,FUNCT
JUMPL AC,.+2
CAILE AC,MAXFN
JRST[ MOVEI TAC,FUNERR
MOVEM TAC,TRAJER
JRST TOFF]
JRST @.+1(AC) ;Perform function
NXTJIF ;0
HOPEN ;1
HCLOSE ;2
NXTJIF ;3
PLACE ;4
NUDGE ;5
SETCH ;6
STOP ;7
SAVE ;10
RESTORE ;11
CENTER ;12
SET.ARM ;13 SET_ARM
WOBBLE ;14
SEARCH ;15
AOJI ;16
SLAVE ;17
HOME ;20
PATH ;21
TOOL ;22
XLIST
SUBTTL SPACEWAR CODE ANGLE MEASUREMENT LOU PAUL
LIST
ANGLES: MOVEI I,5
JFCL 10,.+1
MOVEI L,10
SETCHA: MOVEI K,CHA
TRNE ALT6
MOVEI K,N6CHA
SKIPA
RSTRT: MOVE K,CHAN(I)
SOJL L,REDER
IFE USER,<
PUSHJ P,PREAD
LDB AC,SNUM
CAIN I,5
JRST[ TRNE POTREAD
JRST .+1
CAIL AC,1000
CAILE AC,7000
JRST[ TRC ALT6
JRST SETCHA]
MOVEI K,CHA5
JRST .+1]
ANDI DACVAL,7777
ADDI AC,(DACVAL)
SETZ TAC,
DIV AC,REFTR
ASH AC,-26
MOVE TAC1,AC
TRNE POTREAD
JRST[ ADDI TAC1,=1000
JRST EXON]
ASHC AC,-11 ;LEAVE INDEX INTO NON-LINEAR DIFF TABLE IN AC
LDB MQ,NON(I) ;GET LOWER DIFF
ADDI TAC1,(MQ) ;ADD IT TO READING
AOJ AC,
LDB AC,NON(I) ;GET UPPER DIFFERENCE
SUBI AC,(MQ)
MUL TAC,AC ;INTERPOLATE
ADD TAC1,TAC ;CORRECTED READING
FSC TAC1,226 ;FLOAT
FMPR TAC1,SCALE(I)
FADR TAC1,OFF(I)
>
IFN USER,<MOVE TAC1,TH(I)>
CAIN I,5
JRST[ TRNE ALT6
FADR TAC1,OFF6
FADR TAC1,ROT6
MOVEM TAC1,TH+5
FSBR TAC1,T0+5
MOVSI MQ,(180.0)
CAML TAC1,MQ
JRST[ INCROT: MOVN MQ,MQ
FSC MQ,1
FADRM MQ,ROT6
FADRM MQ,TH+5
JRST THS]
MOVN MQ,MQ
CAMG TAC1,MQ
JRST INCROT
JRST THS]
EXON: MOVEM TAC1,TH(I) ;THETA
THS:
IFN REPORT,<
MOVE MQ,TH(I)
FSBR MQ,T0(I)
SPOS: MOVEM MQ,JER(I)
>
IFE USER,<CAILE I,2>
JRST[ MOVE AC,TH(I)
EXCH AC,THP(I)
FSBR AC,THP(I)
FDVR AC,TDF
MOVNM AC,TD(I)
JRST SERVO]
IFE USER,<
MOVE K,VCHAN(I)
NVEL: PUSHJ P,PREAD
LDB AC,SNUM
ANDI DACVAL,7777
ADDI AC,(DACVAL)
SUB AC,VZ(I)
FSC AC,216
FMPR AC,VSCALE(I)
MOVEM AC,TD(I)
>
XLIST
SUBTTL SPACEWAR CODE SERVO CALC. LOU PAUL
LIST
SERVO: TRNN RUN
JRST[ SOJGE I,RSTRT
POPJ P,]
SERVL: MOVE TAC,TH(I)
FSBR TAC,T0(I) ;POSITION ERROR
MOVEM TAC,ET0(I)
TRNE FINAL
JRST NULL ;MODIFY THETA BY INTEGRAL TERM
MOVE TAC1,T0(I)
FSBR TAC1,T0P(I)
FDVR TAC1,TDF
MOVE AC,TAC1
FSBR AC,TDP(I)
MOVEM TAC1,TDP(I)
FDVR AC,TDF
MOVEM AC,TDD(I)
FMPR AC,CII(I)
MODT0: HRRZ K,BMASK(I)
TDNN K,FBI
JRST [ PDIR:SETZM ET0(I)
FMPR AC,CII(I)
JRST NOBACK]
FADRM TAC,ERRINT(I)
IFE STEP,<
MOVN TAC,TD(I)
FADR TAC,TAC1
>
IFN STEP,<MOVN TAC,TD(I)>
IFN REPORT,<SVEL:MOVNM TAC,VER(I)>
FMPR TAC,KV(I) ;VELOCITY GAIN
FADR AC,TAC
FMPR AC,CII(I)
MOVN TAC,ET0(I)
FMPR TAC,KE(I) ;ERROR GAIN
FADR AC,TAC
MOVN TAC,ERRINT(I)
FMPR TAC,KI(I)
FDVR TAC,CII(I)
FADR AC,TAC
NOBACK: CAIE I,2
FMPR AC,[IRAD:0.0174532925] ;JOINT 3 IS IN INCHES
MOVEM AC,TORE(I)
FADR AC,CI(I) ;GRAVITY TERM
MOVEM AC,T(I) ;ERROR CORRECTION TORQUE
XLIST
SUBTTL SPACEWAR CODE DRIVE CALC. LOU PAUL
LIST
DRIVE:
LS0: TDNN DATWD,BMASK(I)
JRST ISP
MOVE MQ,TD(I)
JUMPN MQ,.+2
MOVN MQ,ET0(I)
FMPR MQ,T(I)
JUMPL MQ,[ MOVM MQ,T(I)
FSBR MQ,F0(I)
JUMPL MQ,[ FDVR MQ,F0(I)
FMPR MQ,V0(I)
FSC MQ,1
FADR MQ,V0(I)
SKIPGE T(I)
MOVN MQ,MQ
JRST CEMF]
MOVSI AC,(1.0)
FSBR AC,PK(I)
FMPR MQ,AC
FMPR MQ,KM(I)
FADR MQ,V0(I)
SKIPG T(I)
MOVN MQ,MQ
JRST CEMF]
SKIPN T(I)
JRST[ SKIPN TD(I)
JRST CEMF
MOVE MQ,V0(I)
SKIPGE TD(I)
MOVN MQ,MQ
JRST CEMF]
JUMPE MQ,NOEX
MOVE MQ,V0(I)
SKIPGE T(I)
MOVN MQ,MQ
NOEX: MOVSI AC,(1.0)
FADR AC,PK(I)
FMPR AC,T(I)
FMPR AC,KM(I)
FADR MQ,AC
CEMF:
MOVE AC,EMF(I)
FMPR AC,TD(I)
MOVSI TAC1,(30.0)
JUMPGE MQ,DRVLT
MOVN AC,AC
DRVLT: FSBR TAC1,AC ;Available drive voltage
CAMGE TAC1,[1.0]
JRST MAXDRV
VELOK: MOVM AC,MQ
FDVR AC,TAC1 ;AC= REL TIME ON
FIX AC,211000 ;1=16
CAML AC,[4000000]
JRST DHO ;too much force stop the arm
CAILE AC,776000
MAXDRV: MOVEI AC,776000
IFN REPORT+TRACK+USER+DEB,<
MOVEM AC,MOTOR(I)
SKIPGE MQ
MOVNS MOTOR(I)
>
TRC AC,400000
HRL AC,I
SETDRV:
IFN TRACK,<SKIPN WALK>
DATAO WIDTH,AC
TDZ DATWD,DMASK(I)
JUMPGE MQ,ISP
TDO DATWD,DMASK(I)
ISP: SKIPE REV(I)
TDC DATWD,DMASK(I)
DATAO ARM,DATWD
SOJGE I,RSTRT
JFCL 10,DHO
IFN TRACK,<SKIPN WALK>
IFN SIMU,< ;IF IN SIMU PREDICT NEXT VALUE OF THETA
MOVEI I,5
PNT: MOVE AC,T0(I) ;PRESENT VALUE
FSC AC,1 ;X 2
FSBR AC,T0P(I) ;PREVIOUS VALUE
MOVEM AC,TH(I) ;IS NEXT VALUE
SOJGE I,PNT
>
IFN REPORT,< ;OUTPUT ERRORS AND DRIVES
HRLI AC,PBLK
HRRI AC,1(DATA)
ADD DATA,[XWD 25,25]
BLT AC,(DATA)
>
SETZM ASTP
TRNE STP
JRST[ MOVEI I,5
MTL: MOVE MQ,TORE(I)
FDVR MQ,F0(I)
SKIPN TD(I)
FSC MQ,-2
FMPR MQ,STQ(I)
FADRM MQ,ASTP
SOJGE I,MTL
MOVE AC,ASTP
CAML AC,SDTQ
JRST[ AOS AC,ASTPP
CAIL AC,2
JRST[ TRZ STP
SUB P,[XWD 1,1]
SETZM ERRINT
MOVE AC,[XWD ERRINT, ERRINT+1]
BLT AC,ERRINT+5
JRST RUDONE]
POPJ P,]
SETZM ASTPP
POPJ P,]
POPJ P,
NULL: SETZB AC,TAC1
TLNN DATWD,NNUL
TRNE WOB
SOJA J,MODT0
TDNN DATWD,GMASK(I)
SOJA J,DRIVE
MOVE MQ,FBI
TDNN MQ,GMASK(I)
SOJA J,PDIR
MOVM MQ,TAC ;|POSITION ERROR|
CAMLE MQ,ERR(I) ;COMPARE POSITION ERROR
JRST MODT0
MOVM MQ,TD(I)
CAMLE MQ,ERR(I)
JRST MODT0
SO: TDZ DATWD,BMASK(I)
IFN REPORT+TRACK+USER+DEB,<SETZM MOTOR(I)>
DATAO ARM,DATWD
HRLZ MQ,I
TRC MQ,400000
DATAO WIDTH,MQ
SOJA J,DRIVE
POPJ P,
XLIST
SUBTTL SPACEWAR CODE LOU PAUL
LIST
NXTH: MOVE AC,[XWD T0,T0P]
BLT AC,T0P+5
TRNE WOB
JRST[ MOVEI I,2
WL1: SOSGE J,WOBCNT(I)
JRST[ MOVEI J,=19
MOVEM J,WOBCNT(I)
JRST WL2]
WL2: MOVE AC,SIN(J)
FMPR AC,WOBMAG
FADRM AC,TFF+3(I)
SOJGE I,WL1
JRST .+1]
TRNE DROP
JRST[ MOVEI I,5
OL: MOVE AC,DELTH(I)
FMPR AC,TDF
FADRM AC,DTH(I)
SOJGE I,OL
JRST JALS]
NTH:
TRNE INCREM
JRST[ MOVE MQ,ETIME
ADDB MQ,TICKS
CAML MQ,NTICKS
JRST[ TRZ INCREM
SKIPN TP
TRO FINAL
MOVEI AC,=600
MOVEM AC,COUNT
SKIPN TP(I)
TLNN DATWD,INT
JRST FIL1
MOVE AC,[XWD NC,CI]
BLT AC,CII+5
FIL1: MOVEI I,5
FIL: MOVE AC,DELTH(I)
FADRB AC,DTH(I)
FADR AC,TFF(I)
MOVEM AC,T0(I)
SETZM DELTH(I)
SOJGE I,FIL
POPJ P,]
SETZ I,
DIV MQ,NTICKS
ASH MQ,-10
FSC MQ,200
MOVSI AC,(6.0)
FMPR AC,MQ
FADR AC,[-15.0]
FMPR AC,MQ
FADR AC,[10.0]
FMPR AC,MQ
FMPR MQ,MQ
FMPR MQ,AC
MOVEI I,5
LDC: MOVE AC,DELTH(I)
FMPR AC,MQ
FADR AC,DTH(I)
FADR AC,TFF(I)
MOVEM AC,T0(I)
SKIPN TP
TLNN DATWD,INT
JRST LDC1
MOVE AC,DCI(I)
FMPR AC,MQ
FADR AC,PC(I)
MOVEM AC,CI(I)
MOVE AC,DCII(I)
FMPR AC,MQ
FADR AC,PCC(I)
MOVEM AC,CII(I)
LDC1: SOJGE I,LDC
POPJ P,]
JALS: MOVEI I,5
JAL: MOVE AC,DTH(I)
FADR AC,TFF(I)
MOVEM AC,T0(I)
SOJGE I,JAL
POPJ P,
EVAL: MOVEI I,5
EL: MOVE J,TP(I) ;POINTS TO KOE5
JUMPE J,NE
MOVE MQ,ETIME
ADDB MQ,TJ(I)
CAML MQ,TN(I) ;NUMBER OF TICKS THIS SEGMENT
JRST[ ;END OF SEGMENT
CAIN I,5
JRST[ ;JOINT 6 HAS CONTROL WORD
MOVE TAC,TICKS
CAMGE TAC,NTICKS
TRO INCREM
LDB TAC,[POINT 3,1(J),2] ;LEVEL
JUMPN TAC,[ ;SWITCH POINT
SKIPG LOOP(TAC)
JRST[ ;INITIALIZE LOOP
LDB AC,[POINT 15,1(J),17]
MOVEM AC,LOOP(TAC) ;COUNT
JRST SETALT]
SOSLE LOOP(TAC) ;IN LOOP
SETALT: TRO ALT
JRST GETNXT]
JRST GETNXT]
GETNXT: TRNE ALT
JRST[ LDB J,[POINT 9,(J),8] ;ALT POINTER
JRST GOT]
LDB J,[POINT 9,(J),17] ;NEXT POINTER
JUMPE J,[SETZM TP(I)
TRO FINAL
MOVEI AC,=600
MOVEM AC,COUNT
MOVE TAC,[XWD NC,CI]
BLT TAC,CI+=11
MOVE AC,A4(I)
ADD AC,A3(I)
ADD AC,A2(I)
ADD AC,A1(I)
ADD AC,A0(I)
XOR AC,SC(I)
FSC AC,0
MOVEM AC,TFF(I)
JRST NE]
GOT: ADDI J,(CBUF)
MOVEM J,TP(I)
SUB MQ,TN(I) ;MICRO SEC INTO NEW SEGMENT
MOVEM MQ,TJ(I) ;INITIALIZE
HRRZ AC,(J)
ASH AC,16
MOVEM AC,TN(I)
CAIN I,5
JRST[
HRRZ AC,1(J)
ADDI AC,(CBUF)
MOVEI MQ,6(AC)
MOVEI K,5
MOVE MQ,(MQ) ;NEW DATWD
FTF: MOVE TAC,GMASK(K)
TDNN TAC,FBI
JRST[ TDNN TAC,MQ ;WAS FREE
JRST FNF ;STILL FREE
MOVE TAC,TH(K)
MOVEM TAC,T0(K)
FSBR TAC,TFF(K)
MOVEM TAC,DTH(K)
MOVNM TAC,DELTH(K)
MOVE TAC,TN+5
ASH TAC,16
MOVEM TAC,NTICKS
SETZM TICKS
TRO INCREM
JRST FNF]
TDNN TAC,MQ ;NOT FREE
JRST[ MOVE TAC,TDD(K) ;NOW IS FREE
CAIE K,2
FMPR TAC,IRAD
FMPR TAC,CII(K)
FMPR TAC,CII(K)
FSBR TAC,T(K)
MOVNM TAC,NC(K)
JRST FNF]
FNF: SOJGE K,FTF
PUSHJ P,UPDC
JRST DDELC]
DDELC: HLRE AC,-3(J)
MOVEM AC,A4(I)
HRRE AC,-3(J)
MOVEM AC,A3(I)
HLRE AC,-2(J)
MOVEM AC,A2(I)
HRRE AC,-2(J)
MOVEM AC,A1(I)
HLLZ AC,-1(J)
MOVEM AC,SC(I)
HRRE AC,-1(J)
MOVEM AC,A0(I)
MOVE MQ,TJ(I)
JRST ELL]
ELL: DIV MQ,TN(I)
TLNE DATWD,INT
CAIE I,5
JRST ELL1
MOVEI K,=11
HLRE TAC,MQ
FSC TAC,211
DECC: MOVE AC,DCI(K)
FMPR AC,TAC
FADR AC,PC(K)
MOVEM AC,CI(K)
SOJGE K,DECC
ELL1: MOVE AC,A4(I)
MUL AC,MQ
ADD AC,A3(I)
MUL AC,MQ
ADD AC,A2(I)
MUL AC,MQ
ADD AC,A1(I)
MUL AC,MQ
ADD AC,A0(I)
XOR AC,SC(I)
FSC AC,0
MOVEM AC,TFF(I)
CAIN I,5
JRST[ MOVE AC,TFF+5
FSBR AC,T0+5
MOVSI MQ,(180.0)
CAML AC,MQ
JRST[ INCT0: FSC MQ,1
FADRM MQ,T0+5
JRST .+1]
MOVN MQ,MQ
CAMG AC,MQ
JRST INCT0
JRST .+1]
NE: SOJGE I,EL
TRZ ALT
POPJ P,
DHO: AOJ I, ;DISASTER HAS OCCURED
LSH I,3 ;JOINT NUMBER
IORI I,EXER
MOVEM I,TRAJER
POP P,I
ZITO: SETZM ERRINT
MOVE AC,[XWD ERRINT,ERRINT+1]
BLT AC,ERRINT+5
TOFF: MOVE AC,STKPTR
MOVEM AC,ERRSTK
DATAO DEVT,[0]
MOVEI L,1000
TRNE RONLY
JRST READED
TRZ RUN+FINAL+DROP+INCREM+HCL+STP+WOB ;STOP THE ARM
SETZM TP
MOVE AC,[XWD TP,TP+1]
BLT AC,TP+5
PUSHJ P,SETSET
MOVNXT: AOS AC,STKPTR
MOVNX: SOJL L,[ LE:
MOVEI TAC,LOOPER
MOVEM TAC,TRAJER
JRST PW]
HLRE TAC,@STKPTR
CAIN TAC,3 ;IS IT A WAIT
JRST MOVNXT
CAIN TAC,SKPERR
JRST[ HRRZ TAC,(AC)
CAMN TAC,TRAJER
JRST[ AOS STKPTR
JRST NEXT]
JRST MOVNXT]
CAIN TAC,SKPNER
JRST[ HRRZ TAC,(AC)
CAME TAC,TRAJER
JRST[ AOS STKPTR
JRST MOVNXT]
JRST NEXT]
CAIN TAC,SKPSER
JRST[ LDB MQ,[POINT 3,(AC),35]
LDB TAC,[POINT 3,TRAJER,35]
CAIN TAC,(MQ)
JRST[ HRRZ TAC,(AC)
ANDI TAC,777770
TDNE TAC,TRAJER
JRST[ AOS STKPTR
JRST NEXT]
JRST MOVNXT]
JRST MOVNXT]
CAIN TAC,JMPREL
JRST[ HRRE TAC,(AC)
ADDI AC,(TAC)
HRRM AC,STKPTR
JRST MOVNX]
PW: CAIL AC,BUF
HRRM AC,JUMPBK
MOVEI AC,WAITIN
MOVEM AC,STKPTR
MOVEI AC,3
MOVEM AC,FUNCT
IFN TRACK,<SKIPN WALK>
DATAO ARM,[2] ;STOP THE ARM
IFN REPORT,<PUSH DATA,[0]>
READED: TRO DTST
IFN DEB,<
MOVEM 17,ACSAV+17
MOVEI 17,ACSAV
BLT 17,ACSAV+16
>
JRST SWDONE
NEXT: TRNE RUN
JRST[ SETZM FUNCT
JRST NXTJIF]
MOVEI L,1000
MOVE AC,FUNCT
CAIE AC,6 ;SET TOUCH
JRST[ MOVE TAC,SOTCHS
MOVEM TAC,SOTCH
TRZ STCH
JUMPE TAC,.+1
TRO STCH
JRST .+1]
IFN TRACK,<SKIPN WALK>
MOVEI DATWD,2
DATAO ARM,DATWD ;STOP THE ARM
INCPIC: AOS AC,STKPTR
PICKUP: SOJL L,LE
HLRE TAC,@STKPTR
CAIN TAC,JMPABS
JRST[JMPA:HRRZ TAC,@STKPTR
MOVEM TAC,STKPTR
HLRZ TAC,@STKPTR
CAIN TAC,3
JRST INCPIC
JRST PICKUP]
CAIN TAC,SKPERR
JRST[ HRRZ TAC,@STKPTR
CAMN TAC,TRAJER
AOS STKPTR
JRST INCPIC]
CAIN TAC,SKPNER
JRST[ HRRZ TAC,@STKPTR
CAME TAC,TRAJER
AOS STKPTR
JRST INCPIC]
CAIN TAC,SKPSER
JRST[ LDB MQ,[POINT 3,@STKPTR,35]
LDB TAC,[POINT 3,TRAJER,35]
CAIN TAC,(MQ)
JRST[ HRRZ TAC,@STKPTR
ANDI TAC,777770
TDNE TAC,TRAJER
AOS STKPTR
JRST INCPIC]
JRST INCPIC]
CAIN TAC,JMPREL
JRST[ HRRE TAC,@STKPTR
ADD TAC,STKPTR
HRRM TAC,STKPTR
JRST PICKUP]
SETZM TRAJER
MOVEI I,100
MOVEM I,COUNT
TRZE TAC,MERGE
TRO BOTH
MOVEM TAC,FUNCT
CAIN TAC,3 ;IS IT A WAIT
JRST[ MOVE TAC,STKPTR
CAIL TAC,BUF
HRRM TAC,JUMPBK
MOVEI TAC,WAITIN
MOVEM TAC,STKPTR
IFN REPORT,<PUSH DATA,[0]>
TRO DTST
PUSHJ P,SWDONE]
MARK:
IFN REPORT,<
PUSH DATA,[<SIXBIT/NEXT/>+1]
MOVE TAC,FUNCT
HRL TAC,0
PUSH DATA,TAC
>
; JRST DISP
SWDONE:
MOVEM BITSAV
MOVEM DATWD,DATWDS
MOVEM CBUF,CBUFS
IFN REPORT+TRACK,<
MOVEM DATA,DATPT
>
IFE USER,<
IFN TRACK,<
SKIPE WALK
JRST @STRADR-1
>
CALLI 400024
>
IFN USER,<
JRST @STRADR-1
>
SETSET: MOVEI I,5
TCHK: MOVE J,TP(I)
JUMPE J,ATEND
SETZM TJ(I)
SETZM TN(I)
GNN: LDB J,[POINT 9,(J),17]
ADDI J,(CBUF)
CAIN J,(CBUF)
JRST[ MOVEM J,TP(I)
JRST ATEND]
JRST GNN
ATEND: SOJGE I,TCHK
PUSHJ P,EVAL
MOVEI I,5
FDTH: MOVE AC,TH(I)
MOVEM AC,T0(I)
FSBR AC,TFF(I)
MOVEM AC,DTH(I)
SETZM DELTH(I)
SETZM TDP(I)
SETZM TDD(I)
SOJGE I,FDTH
POPJ P,
UPDC: MOVE TAC,[XWD NC,PC]
BLT TAC,PCC+5
HRL AC,AC
HRRI AC,NC
BLT AC,NC+6
MOVE DATWD,NC+6
TLNE DATWD,NXTFN
JRST[ AOS AC,STKPTR
HLRE AC,(AC)
MOVEM AC,FUNCT
JRST .+1]
HRRZM DATWD,FBI
MOVEI K,5
UPL: MOVE AC,NC(K)
HLLZM AC,NC(K)
HRLZM AC,NCC(K)
MOVE AC,MOTARM(K)
FADRB AC,NCC(K)
TLNN DATWD,INT
JRST UPL1
FSBR AC,PCC(K)
MOVEM AC,DCII(K)
MOVE AC,NC(K)
FSBR AC,PC(K)
MOVEM AC,DCI(K)
UPL1: TDNE DATWD,BMASK(K)
TDO DATWD,BMASK(K)
SOJGE K,UPL
TLNE DATWD,INT
POPJ P,
MOVE AC,[XWD NC,CI]
BLT AC,CII+5
POPJ P,
PREAD:
IFN TRACK,<
SKIPE WALK
JRST[GNPR:AOBJP DATA,[ MOVE AC,[IOWD DBL,DATO]
MOVEM AC,DATPT
MOVE DATA,DATPT
SETZ TAC,
INPUT 15,1
JRST GNPR]
MOVE DACVAL,(DATA)
JRST @READFN]
>
MOVEI TAC,10
MOVEM TAC,DATMSD
DACRED: SOSGE DATMSD
JRST[REDER: MOVEI TAC,READER
MOVEM TAC,TRAJER
JRST TOFF]
DACST: CONO DB,4250
CONO AD,(K)
NREAD: MOVEI TAC,30
CONSO DB,1000
SOJGE TAC,.-1
JUMPL TAC,[DACDIE:CONO AD,4000(K) ;STOP DAC
TRO REREAD
IFN DEB,<MOVEM DACVAL,DACERR>
MOVEI TAC,12
WRH: SOJGE TAC,WRH
JRST DACRED]
DATAI DB,DACVAL
CONSZ DB,10000
JRST DACDIE
XOR DACVAL,SBMSK
IFN TRACK,<PUSH DATA,DACVAL>
JRST @READFN
FREAD: ADDI K,770000
IFN TRACK,<SKIPN WALK>
CONO AD,4000(K) ;STOP AD ON NEXT CHANNEL
POPJ P,
READFN: FREAD
XLIST
SUBTTL CONSTANTS LOU PAUL
LIST
NON: POINT 12,INNER(AC),11
POINT 12,INNER(AC),23
POINT 12,INNER(AC),35
POINT 12,OUTER(AC),11
POINT 12,OUTER(AC),23
POINT 12,OUTER(AC),35
ERR: 0.05
0.05
0.01
0.1
0.1
0.7
IFN SIMU,<
TH-1
ARM.TH: .+4
>
TH: -180.0 ;THE NEXT 7 LOCATIONS ARE BLOCK TRANSFERED.
-90.0
12.0
-90.0
90.0
0.0
IFN SIMU,<ARM.GR:>
HAND: 3.0
TDD: BLOCK 6
ERRINT: 0
0
0
0
0
0
DTH: BLOCK 6
TD: BLOCK 6
T0P: BLOCK 6
TDP: BLOCK 6
ET0: BLOCK 6
T0: -150.0
-75.0
14.0
-90.0
60.0
0.0000
TFF: -150.0
-75.0
14.0
-90.0
60.0
0.0000
DELTH: BLOCK 6 ;THIS AND THE NEXT ARE BLT ED TOGETHER
DDTH: 0
0
0
0
0
0
NTICKS: 0
DTHS: BLOCK =30
TORE: BLOCK 6
T: BLOCK 6
IFN REPORT,<
PBLK: <SIXBIT/THETA/>+6
JER: BLOCK 6
<SIXBIT/VEL/>+6
VER: BLOCK 6
<SIXBIT/DAC/>+6
>
IFN USER+REPORT+TRACK+DEB,<MOTOR: BLOCK 6>
FBI: 252502
VZ: BLOCK 3
IFN TACH,<
VDTH: BLOCK 3
>
THP: BLOCK 6
MOTARM: 840.0
900.0
70.0
55.0
55.0
64.0
STQ: BLOCK 6
SDTQ: 0
ASTP: 0
ASTPP: 0
PC: BLOCK 6
PCC: BLOCK 6
DCI: BLOCK 6
DCII: BLOCK 6
CI: 0
0
0
0
0
0
CII: 1000.0
1200.0
100.0
100.0
100.0
100.0
NC: 0
0
0
0
0
0
NCC: 1000.0
1200.0
100.0
100.0
100.0
100.0
IFN DEB,<DACERR: 0>
REFTR: 0
ETIME: =16667
TDF: 1.0
IFN DEB,<
ETIM: 0
ACSAV: BLOCK 20
>
PDL: DHO
BLOCK PDLL
LTIME: 0
IFN SIMU,<ARM.FU:>
FUNCT: 0
ROT6: 0
TRAJER: 0
DOING: 0
DATMSD: 0
CBUFS: 0
DATWDS: 0
SNUM: POINT 12,DACVAL,23
SBMSK: 400040004000
REV: 0
0
-1
0
-1
-1
DMASK: 400000
100000
20000
4000
1000
200
BMASK: XWD 400000,200000
XWD 200000, 40000
XWD 100000, 10000
XWD 40000, 2000
XWD 20000, 400
XWD 10000, 100
GMASK: 200000
40000
10000
2000
400
100
VCHAN: 52B23
53B23
54B23
CHAN: 55B23
56B23
57B23
60B23
61B23
GAIN: MOVEI I,5
SETGN: MOVE AC,KV(I)
FMPR AC,AC
FMPR AC,THIRD
MOVEM AC,KE(I)
FMPR AC,KV(I)
FMPR AC,SIXTH
MOVEM AC,KI(I)
SOJGE I,SETGN
SETNM: MOVE 1,[SIXBIT/[ARM]/]
CALLI 1,400002
MOVE I,RET
MOVEM I,SETNM
JRST SETNM
RET: POPJ P,
THIRD: 0.3
SIXTH: 0.1
BLOCK =20
0
WAITIN: XWD 3,0
JUMPBK: XWD JMPABS,WAITIN
STKPTR: WAITIN
ERRSTK: 0
BITSAV: 0
COUNT: 0
STWT: 0
HCNT: 0
TP: 0
0
0
0
0
0
TJ: BLOCK 6
LOOP: BLOCK 10
TN: BLOCK 6
SC: BLOCK 6
A0: BLOCK 6
A1: BLOCK 6
A2: BLOCK 6
A3: BLOCK 6
A4: BLOCK 6
BLOCKS: 0
NXTBUF: 0
BUF: 0
BLOCK 1000
BUF2: 0
BLOCK 1000
IFN REPORT+TRACK,<
DATIWD: 0
0
DATPT: 0
DATO: BLOCK DBL+100
>
IFN DYNAMICS,<
DDAT: <SIXBIT/FORD/>+6
FOR: BLOCK 6
<SIXBIT/BACK/>+6
BAK: BLOCK 6
>